prev_error_x, prev_error_y = 0,0  # 声明全局变量


def calculate_next_velocity(dx, dy, vx, vy, kp, ki, kd):
    """计算下一步无人机速度"""
    global prev_error_x, prev_error_y
    # 计算误差信号
    error_x = dx
    error_y = dy

    # 计算积分信号
    ix = 0.5 * (error_x + prev_error_x)
    iy = 0.5 * (error_y + prev_error_y)

    # 计算微分信号
    dx = error_x - prev_error_x
    dy = error_y - prev_error_y

    # 计算修正速度
    vx += kp * error_x + ki * ix + kd * dx
    vy += kp * error_y + ki * iy + kd * dy

    # 更新上一次的误差
    prev_error_x = error_x
    prev_error_y = error_y

    return vx, vy


# 定义主程序
if __name__ == '__main__':

    # 初始化无人机位置和速度
    x = y = vx = vy = 0

    # 设置目标点
    target_x = 100
    target_y = 200

    # 设置反馈控制参数
    kp = 100
    ki = 10
    kd = 100

    # 模拟无人机移动过程
    for i in range(10000):
        # 计算到目标点的距离
        dx = target_x - x
        dy = target_y - y

        # 计算下一步的无人机速度
        vx, vy = calculate_next_velocity(dx, dy, vx, vy, kp, ki, kd)

        # 更新无人机位置
        x += vx
        y += vy

        print(f"vx={vx} vy={vy} x={x}, y={y}")
